from __future__ import print_function
from GeometricalObjects import Route
from GeometricalObjects import distance
class SearchDijkstra(object):

	def __init__(self, start_point, end_point, nb_steps, max_x, max_y):
		self.start_point = start_point
		self.end_point = end_point
		self.nb_steps = nb_steps
		self.best_route = Route(self.start_point, self.end_point, nb_steps, max_x, max_y)

	def launch_dijkstra(self, points):
		for p in points:
			p.previous_steps = [{ "previous_point":self.start_point, "length_to_point":distance(self.start_point, p) } ]
			if self.nb_steps - 1:
				for r in range(self.nb_steps-1):
					p.previous_steps.append({"previous_point":None , "length_to_point":self.best_route.length()} )
			p.last_step = {"previous_point":None, "length_to_point": self.best_route.length()}
		for idx,p in enumerate(points):
			self.dijkstra( p, points[:idx] + points[idx+1:], 1)
		p = points[0]
		p.last_step["previous_point"] = p.previous_steps[self.nb_steps-1]["previous_point"]
		p.last_step["length_to_point"] = p.previous_steps[self.nb_steps-1]["length_to_point"]+distance(p.last_step["previous_point"],self.end_point)
		best_point, best_length = p, p.last_step["length_to_point"] 
		for p in points[1:]:
			p.last_step["previous_point"] = p.previous_steps[self.nb_steps-1]["previous_point"]
			p.last_step["length_to_point"] = p.previous_steps[self.nb_steps-1]["length_to_point"]+distance(p.last_step["previous_point"],self.end_point)
			if p.last_step["length_to_point"] < best_length:
				best_point, best_length = p, p.last_step["length_to_point"] 
		best_route = [ best_point]
		p = best_point
		for r in range(self.nb_steps-1,0,-1):
			p = p.previous_steps[r]["previous_point"]
			best_route.append(p)
		best_route.reverse()
		for p in best_route: self.best_route.add_waypoint(p)
		self.best_route.print_route()

	def dijkstra(self, pivot_point, points, step):
		if step == self.nb_steps:
			return
		length_to_pivot = pivot_point.previous_steps[step-1]["length_to_point"]
		for p in points:
			#A->...->pivot->p
			best_step_point, best_step_length = p.previous_steps[step]["previous_point"] , p.previous_steps[step]["length_to_point"]
			new_length = length_to_pivot + distance(pivot_point, p)
			if new_length < best_step_length:
				p.previous_steps[step]["previous_point"] = pivot_point
				p.previous_steps[step]["length_to_point"] = new_length
		for idx,p in enumerate(points):
			self.dijkstra(p, points[:idx] + points[idx+1:], step+1)

import copy
class Searcher_Basic(object):

	def __init__(self):
		pass

	def launch_dijkstra(self, start_point, end_point, nb_steps, points, max_x, max_y, need_to_sort = False):
		try:
			if self.best_route:
				#print("Starting search with preliminary best route found:")
				#self.best_route.print_route()
				pass
		except:
			self.best_route = Route(start_point, end_point, nb_steps, max_x, max_y)
		self.route = Route(start_point, end_point, nb_steps, max_x, max_y)
		self.points_added = 0
		self.best_route_length = self.best_route.length()
		self.need_to_sort = need_to_sort
		self.tuned_dijkstra(0, nb_steps, points)
		#tuned_dijkstra(0, nb_steps, points, route, best_route)
		#best_route.print_route()
		return self.best_route

	def sort_with_reference(self, points):
		nb_points = len(points)
		ref_point = self.route.current_point
		for idx in range(nb_points):
			closest, closest_dist = idx, distance(ref_point, points[idx])
			for idx2 in range(idx+1, nb_points):
				new_idx, new_dist = idx2, distance(ref_point, points[idx2])
				if new_dist < closest_dist:
					closest, closest_dist = new_idx, new_dist
			points[idx] ,  points[closest] = points[closest] , points[idx]
		

	def tuned_dijkstra(self, current_step, max_steps, points):
		if current_step == max_steps:
			return
		if self.route.current_length() > self.best_route.length():
			return
		#print("step {0}".format(current_step))
		my_sorted_points = (points)
		#if self.need_to_sort: self.sort_with_reference(my_sorted_points)	
		for idx, p in enumerate(my_sorted_points):
			self.route.add_waypoint(p)
			self.points_added += 1
			if current_step != max_steps - 1:
				self.tuned_dijkstra( current_step + 1, max_steps, my_sorted_points[:idx]+my_sorted_points[idx+1:])
			else:
				new_length = self.route.length()
				if new_length < self.best_route_length:
					#my_best_route = copy.deepcopy(route)
					self.best_route = copy.deepcopy(self.route)
					ratio = self.best_route_length / float(new_length) * 100
					if ratio < 200: ratio -= 100
					#print("New best route found: {0} vs {1} = improved by {2}%".format(self.best_route_length, new_length, ratio)) 
					self.best_route_length = self.best_route.length()
					#my_best_route.print_route()
				pass
			self.route.remove_last_waypoints(current_step)	
		#print("Finishing Dijkstra with best_route @ step {0}".format(current_step))
		#my_best_route.print_route()
		return
	
